import lejos.nxt.*;

public class WaypointDrive extends Behavior 
{
	private int[] waypoints;
	private int waypointIndex;

	public WaypointDrive( String name, int LCDrow, Behavior b, int[] waypoints)
	{
		super(name, LCDrow, b);
		this.waypoints = waypoints;
	}

	public void run() 
	{
		while (true)
		{
			super.run();
			
			if(waypoints.length > waypointIndex + 1)
			{
				suppress();
				//MotorPort.A.controlMotor(100, 1); // light on

				LCD.drawString("" + waypoints[waypointIndex] + ", " + waypoints[waypointIndex+1] + "         ",0, 4);
				//stopCar();
				goTo(waypoints[waypointIndex], waypoints[waypointIndex+1]);
				if(Math.abs(NavigationCar.getX()-waypoints[waypointIndex]) < 5 && Math.abs(NavigationCar.getY()-waypoints[waypointIndex+1]) < 5)
					waypointIndex = waypointIndex + 2;

				//drawString("          ");
				//MotorPort.A.controlMotor(0, 1); // light off
				release();
			}
			else {
				LCD.drawString("Done                              ",0,4);
				break;
			}
		}
	}
}


